//
// Created by gii on 22-12-17.
//

#ifndef DIJKSTRA_SRC_PLANNER_TOOL_SRC_DIJKSTRA_H_
#define DIJKSTRA_SRC_PLANNER_TOOL_SRC_DIJKSTRA_H_

#include "geometry_msgs/PointStamped.h"
#include "map"
#include "station.h"
#include "visualization_msgs/MarkerArray.h"

#define NONE_ID -1
#define INF 9999999999
#define MAX_TRY_TIMES 5000
typedef int ID;

// struct Point : public geometry_msgs::Point {
//  Point() = default
//  __attribute__((unused)) Point(double x_, double y_) {
//    x = x_;
//    y = y_;
//  }
//  double operator-(Point &p) { return std::hypot(x - p.x, y - p.y); }
//};

struct Node {
  Station station;

  std::set<ID> children;

  double operator-(Node &n) { return station.point - n.station.point; }
};

struct Map : std::map<ID, Node> {
  ID findNode(Point p) {
    double min_dist = INF;
    ID id = NONE_ID;
    for (auto &node : *this) {
      double dist = node.second.station.point - p;
      if (dist < min_dist && dist < 0.5) {
        min_dist = dist;
        id = node.first;
      }
    }
    return id;
  }

  bool containNode(ID id) { return id != NONE_ID && this->find(id) != end(); }
};
typedef std::vector<ID> Path;

class Dijkstra {
  struct Cost {
    ID parent{NONE_ID};
    double dist{INF};
  };
  typedef std::map<ID, Cost> CostMap;

  ID goal_{NONE_ID};
  CostMap cost_map_;
  std::vector<ID> edge_;
  bool found_goal_flag_{false};

  bool expand(Map &map) {
    // not node more, return
    if (edge_.empty()) { return false; }

    std::vector<ID> tmp_edge = edge_;
    edge_.clear();

    for (auto &id : tmp_edge) {
      double current_dist = cost_map_[id].dist;
      for (auto &child_id : map[id].children) {
        // child node is not touched, add to edge
        if (cost_map_.find(child_id) == cost_map_.end()) { edge_.push_back(child_id); }

        double tmp = current_dist + (map[id] - map[child_id]);
        if (tmp < cost_map_[child_id].dist) {
          cost_map_[child_id].parent = id;
          cost_map_[child_id].dist = tmp;
        }

        if (child_id == goal_) { found_goal_flag_ = true; }
      }
    }
    return true;
  }

 public:
  explicit Dijkstra() = default;

  bool findPath(Map &map, ID start, ID goal, Path &path) {
    if (map.find(start) == map.end()) { return false; }
    if (map.find(goal) == map.end()) { return false; }
    if (start == goal) { return false; }

    // clear
    edge_.clear();
    cost_map_.clear();
    found_goal_flag_ = false;

    // init
    edge_.push_back(start);
    cost_map_[start].dist = 0;
    goal_ = goal;

    // calc cost of node
    int try_times = 0;
    while (expand(map)) {
      if (found_goal_flag_) {
        for (auto &id : edge_) {
          // have cost of edge which less than cost of goal, so continue to expand;
          if (cost_map_[id].dist < cost_map_[goal_].dist) { continue; }
        }

        // found && no more cost of edge less than cost of goal
        break;
      }

      if (try_times++ > MAX_TRY_TIMES) { return false; }
    }

    if (!found_goal_flag_) { return false; }

    try_times = 0;
    ID node_id = goal;
    while (node_id != NONE_ID) {
      path.push_back(node_id);

      if (node_id == start) { return true; }

      node_id = cost_map_[node_id].parent;

      if (try_times++ > MAX_TRY_TIMES) { return false; }
    }

    return false;
  }
};

#endif  // DIJKSTRA_SRC_PLANNER_TOOL_SRC_DIJKSTRA_H_
